%% 四旋翼执行器故障诊断仿真（基于EKF）
clc; clear; close all;

% 仿真参数
dt = 0.01;          % 时间步长
T = 10;             % 总时长
t = 0:dt:T;         % 时间向量
N = length(t);

% 无人机参数
Ix = 0.05; Iy = 0.05; Iz = 0.1;  % 转动惯量 (kg·m^2)
l = 0.2;            % 旋翼到质心距离 (m)
kappa = 0.01;       % 旋翼扭矩系数

% 初始状态 [phi, theta, psi, p, q, r]
x_true = zeros(6,1);    % 真实状态
x_est = zeros(6,1);     % 估计状态

% 协方差矩阵
P = eye(6);             % 状态协方差
Q = diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1]); % 过程噪声
R = diag([0.01, 0.01, 0.01]);    % 测量噪声（假设仅测量欧拉角）

% 存储变量
residuals = zeros(3,N); % 残差
fault_detected = zeros(1,N);

% 控制输入（四旋翼推力指令）
F_cmd = [1; 1; 1; 1] * 5; % 正常推力 (N)

% 故障注入参数
fault_time = 5;         % 故障发生时间 (s)
fault_rotor = 1;        % 故障旋翼编号（1-4）
fault_type = 'partial'; % 'complete'（完全失效）或 'partial'（部分失效）
fault_alpha = 0.5;      % 部分失效时的推力损失比例（50%）

%% 主仿真循环
for k = 1:N
    % --- 故障注入 ---
    F = F_cmd;
    if t(k) >= fault_time
        switch fault_type
            case 'complete'
                F(fault_rotor) = 0;  % 完全失效
            case 'partial'
                F(fault_rotor) = (1 - fault_alpha) * F_cmd(fault_rotor); % 部分失效
        end
    end
    
    % --- 真实动力学更新 ---
    % 控制力矩
    tau_phi = l * (F(2) - F(4));
    tau_theta = l * (F(3) - F(1));
    tau_psi = kappa * (F(1) - F(2) + F(3) - F(4));
    
    % 角加速度
    p = x_true(4); q = x_true(5); r = x_true(6);
    p_dot = (tau_phi + (Iy - Iz)*q*r) / Ix;
    q_dot = (tau_theta + (Iz - Ix)*p*r) / Iy;
    r_dot = (tau_psi + (Ix - Iy)*p*q) / Iz;
    
    % 状态更新（欧拉积分）
    x_true(4:6) = x_true(4:6) + [p_dot; q_dot; r_dot] * dt;
    x_true(1:3) = x_true(1:3) + [p; q; r] * dt;
    
    % 添加过程噪声
    x_true = x_true + sqrt(Q) * randn(6,1) * dt;
    
    % --- EKF预测步骤 ---
    % 状态预测（与真实动力学相同）
    p_est = x_est(4); q_est = x_est(5); r_est = x_est(6);
    p_dot_est = (tau_phi + (Iy - Iz)*q_est*r_est) / Ix;
    q_dot_est = (tau_theta + (Iz - Ix)*p_est*r_est) / Iy;
    r_dot_est = (tau_psi + (Ix - Iy)*p_est*q_est) / Iz;
    
    x_pred = x_est + [p_est; q_est; r_est; p_dot_est; q_dot_est; r_dot_est] * dt;
    
    % 雅可比矩阵 F = df/dx
    F_jac = [...
        0, 0, 0, 1, 0, 0;
        0, 0, 0, 0, 1, 0;
        0, 0, 0, 0, 0, 1;
        0, 0, 0, 0, (Iy-Iz)*r_est/Ix, (Iy-Iz)*q_est/Ix;
        0, 0, 0, (Iz-Ix)*r_est/Iy, 0, (Iz-Ix)*p_est/Iy;
        0, 0, 0, (Ix-Iy)*q_est/Iz, (Ix-Iy)*p_est/Iz, 0];
    
    % 协方差预测
    P_pred = F_jac * P * F_jac' + Q;
    
    % --- EKF更新步骤 ---
    % 测量模型（假设直接测量欧拉角）
    y_true = x_true(1:3) + sqrt(R) * randn(3,1); % 带噪声的测量
    H = [eye(3), zeros(3)]; % 测量雅可比
    
    % 卡尔曼增益
    K = P_pred * H' / (H * P_pred * H' + R);
    
    % 状态更新
    x_est = x_pred + K * (y_true - x_pred(1:3));
    P = (eye(6) - K * H) * P_pred;
    
    % --- 残差计算与故障检测 ---
    residuals(:,k) = y_true - x_est(1:3);
    
    % 卡方检测（残差马氏距离）
    S = H * P_pred * H' + R;
    mahalanobis_dist(k) = residuals(:,k)' / S * residuals(:,k);
    
    % 阈值（95%置信度的卡方分布，自由度为3）
    chi2_threshold = 7.815; 
    if mahalanobis_dist(k) > chi2_threshold
        fault_detected(k) = 1;
    end
end

%% 结果可视化
figure;
subplot(3,1,1);
plot(t, residuals(1,:)); hold on;
plot(t, fault_detected * 0.1, 'r-', 'LineWidth', 2);
title('滚转残差 \phi');
xlabel('时间 (s)'); ylabel('残差');

subplot(3,1,2);
plot(t, residuals(2,:)), hold on;
plot(t, fault_detected * 0.1, 'r-', 'LineWidth', 2);
title('俯仰残差 \theta');
xlabel('时间 (s)'); ylabel('残差');

subplot(3,1,3);
plot(t, residuals(3,:)), hold on;
plot(t, fault_detected * 0.1, 'r-', 'LineWidth', 2);
title('偏航残差 \psi');
xlabel('时间 (s)'); ylabel('残差');

figure;
plot(t, mahalanobis_dist), hold on;
plot(t, chi2_threshold * ones(size(t)), 'r--');
title('残差马氏距离 vs 卡方阈值');
xlabel('时间 (s)'); ylabel('马氏距离');
legend('残差', '阈值');